/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.spot.mechanism.WallHugger;

import com.grt192.controller.pid.PIDOutput;
import com.grt192.controller.pid.PIDSource;
import com.grt192.core.CommandArray;
import com.grt192.core.Mechanism;
import com.grt192.spot.actuator.GRTDemoLED;
import com.grt192.spot.mechanism.hauntedHouse.StatusLight;
import com.grt192.spot.sensor.GRTSonar;
import com.iaroc.irobot.IRobotCreate;

/**
 * A Distancing loop actuator and sensor abstraction
 * @author ajc
 */
public class PIDDistanceDemoMech extends Mechanism implements PIDSource, PIDOutput {

    private final IRobotCreate irc;
    private final GRTSonar s;
    private final StatusLight light;

    public PIDDistanceDemoMech(IRobotCreate irc, GRTSonar s, StatusLight light) {
        this.irc = irc;
        this.s = s;
        this.light = light;
        light.rawColor(GRTDemoLED.Color.ORANGE);
        setPrinting(true);
    }

    /**
     * Returns a distance for a PID loop to calculate with
     * @return
     */
    public double pidGet() {
        double distance = s.getDistance();
        log("Distance: \t\t\t " + distance);
        return distance;
    }

    /**
     * Drive the robot at a proportional speed
     * @param result
     */
    public void pidWrite(double result) {
        log("PIDWrite:\t " + result + " \t" + (int) result);
        colorLight(result);
        if (irc != null) {
            irc.driveDirect((int) result, (int) result);
        }
    }

    /**
     * Color the light white with a brightness proportional to robot speed
     * @param robotspeed
     */
    private void colorLight(double robotspeed) {
        robotspeed /= 5;
        robotspeed += 100;
//        log("color" + )
        light.rawColor(new CommandArray(robotspeed, robotspeed, robotspeed));
    }
}
